#include "hand.h"

float Theta0 = 0, Theta1 = 90, Theta2 = 90, Theta3 = 90;
#define PI 3.14159

void Hand_GpioInit()
{
    IoTGpioInit(P1_GPIO);
    IoTGpioInit(P2_GPIO);
    IoTGpioInit(P3_GPIO);
    IoTGpioInit(P4_GPIO);
    IoTGpioInit(P6_GPIO);
    IoSetFunc(P1_GPIO, 0);
    IoSetFunc(P2_GPIO, 0);
    IoSetFunc(P3_GPIO, 0);
    IoSetFunc(P4_GPIO, 0);
    IoSetFunc(P6_GPIO, 0);
    IoTGpioSetDir(P1_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(P2_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(P3_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(P4_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(P6_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(P1_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(P2_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(P3_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(P4_GPIO, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(P6_GPIO, IOT_GPIO_VALUE0);
}

void P1_set_angle(float angle)
{
    int i;
    unsigned int duty = (-100 * angle / 9) + 1500;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(P1_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(P1_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(P1_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    }
}

void P2_set_angle(float angle)
{
    int i;
    unsigned int duty = (100 * angle / 9) + 500;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(P2_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(P2_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(P2_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    }
}

void P3_set_angle(float angle)
{
    int i;
    unsigned int duty = (100 * angle / 9) + 500;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(P3_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(P3_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(P3_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    }
}

void P4_set_angle(float angle)
{
    int i;
    unsigned int duty = (-100 * angle / 9) + 1500;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(P4_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(P4_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(P4_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    }
}

void P6_set_angle(float angle)
{
    int i;
    unsigned int duty = (-100 * angle / 9) + 1500;
    unsigned int time = 20000;
    for (i = 0; i < COUNT; i++)
    {
        IoTGpioSetDir(P6_GPIO, IOT_GPIO_DIR_OUT);
        IoTGpioSetOutputVal(P6_GPIO, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(P6_GPIO, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    }
}

const int l0 = 100, l1 = 100, l2 = 100;

uint8_t Kinematic_Analysis(float x, float y, float Alpha)
{
    float m, n, k, a, b, c, s1ps2;
    m = l2 * cos(Alpha) - x;
    n = l2 * sin(Alpha) - y;
    k = (l1 * l1 - l0 * l0 - m * m - n * n) / 2 / l0;
    a = m * m + n * n;
    b = -2 * n * k;
    c = k * k - m * m;

    if (b * b - 4 * a * c < 0)
        return 0;

    Theta1 = (-b + sqrt(b * b - 4 * a * c)) / 2 / a;
    Theta1 = asin(Theta1) * 180 / PI;

    if (Theta1 > 90)
        Theta1 = 90;
    if (Theta1 < -90)
        Theta1 = -90;

    k = (l0 * l0 - l1 * l1 - m * m - n * n) / 2 / l1;
    a = m * m + n * n;
    b = -2 * n * k;
    c = k * k - m * m;

    if (b * b - 4 * a * c < 0)
        return 0;

    s1ps2 = (-b - sqrt(b * b - 4 * a * c)) / 2 / a;
    s1ps2 = asin(s1ps2) * 180 / PI;

    if (s1ps2 > 90)
        Theta2 = 90;
    if (s1ps2 < -90)
        Theta2 = -90;

    Theta2 = s1ps2 - Theta1;
    if (Theta2 > 90)
        Theta2 = 90;
    if (Theta2 < -90)
        Theta2 = -90;

    Theta3 = Alpha * 180 / PI - Theta1 - Theta2;
    if (Theta3 > 90)
        Theta3 = 90;
    if (Theta3 < -90)
        Theta3 = -90;

    return 1;
}

extern uint8_t RobotMode;

static void *HandTask(void *parame)
{
    (void)parame;
    printf("Hand task start\r\n");
    Theta1 = 135;
    Theta2 = 135;
    Theta3 = 45;
    while (1)
    {
        if (RobotMode == MONITOR_MODE)
        {
            Theta1 = 45;
            Theta2 = 180;
            Theta3 = 90;
        }
        P1_set_angle(Theta0);
        P2_set_angle(Theta1);
        P3_set_angle(Theta2);
        P4_set_angle(Theta3);
        osDelay(2);
    }
}

void HandEntry(void)
{
    osThreadAttr_t attr;
    Hand_GpioInit();
    attr.name = "HandTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = HAND_TASK_STACK_SIZE;
    attr.priority = 24;

    if (osThreadNew((osThreadFunc_t)HandTask, NULL, &attr) == NULL)
    {
        printf("[HandEntry] Falied to create HandTask!\n");
    }
}

APP_FEATURE_INIT(HandEntry);